/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file rssiChain.cxx
  \brief class implemenation for RSSI chaining

  \author Cory Dixon
  $Date: 2005/02/12 00:20:45 $
*/
#include <iostream>

#include "rssiChain.h"
#include "utilities.h"
#include "color.h"

/*<-------Defines--------->*/
	// [s]
	#define CHAINING_LOOP_PERIOD 3.0	

	// default control gains, [m/s] and [m^2/dBm/s]
	#define ORBIT_SPEED      5.0
	#define DEFAULT_GAIN_VAL 10.0

	// define this to let piccolo do the orbital control, otherwise we
	// use the LGVF controller
	#define USE_AUTOPILOT_ORBIT

	// which RSSI values to use, ones I measured or that the neighbor measured
	#define LOCAL_RSSI 
	#undef  REMOTE_RSSI

	#define MIN_RSSI -95.0
	#define Cmax      54.0
	#define Ae         0.05
	#define So         0.0

/*<------End Defines------>*/

/*<---Global Variables---->*/
	// externs
	extern bool verbose;
	extern bool logging;
	extern String logDir;

	// local
	int myVerbose = true;
	//int myVerbose = verbose;
/*<-End Global Variables-->*/


RSSIChain::RSSIChain(String name) : ThreadedPipe(name)
{
	last_chaining_loop.stamp();
	last_orbit_ts.stamp();

	orbit_point.lat = NAN;
	orbit_point.lon = NAN;
	orbit_point.alt = NAN;

	control_point.x = NAN;
	control_point.y = NAN;
	control_point.z = NAN;

	for( int i=0; i<MAX_NUM_RSSI; i++) {
		ecef[i].x = NAN;
		ecef[i].y = NAN;
		ecef[i].z = NAN;
	}

	Vcp.x         = 0.0;
	Vcp.y         = 0.0;
	Vcp.z         = 0.0;

	useIND      = 0;
	controlMode = MAXIMIN_CONTROLLER; 

	for( int i=0; i<MAX_CLIENTS; i++) {
		clientRSSI_ind[i]     = -1;
		rssiClient[i].avgRSSI = NAN;
		rssiClient[i].rssiVEC.x = 0;;
		rssiClient[i].rssiVEC.y = 0;;
		rssiClient[i].rssiVEC.z = 0;;
	}

	// orbit speed is in [m/s]
	gains.vmax = ORBIT_SPEED;
	gains.kv   = DEFAULT_GAIN_VAL;

	cout << GREEN << "---------->>>>>      " YELLOW "CHAIN" GREEN "      <<<<<----------"  << ATTROFF<< endl;

	/*--------- log ----------*/
	if(logging) {
		log_name = generteNumberedFile( logDir, "rssi_chain_");
	}
}

bool RSSIChain::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);

	// initialize chain variables
	resetChain();

	// subscribe myself to my GPS stream from piccolo
	Client * me = shmPtr->clients.getClient(0);
	shmPtr->clients.subscribe(me, CMD_TELEM);

	last_chaining_loop.stamp();
	last_orbit_ts.stamp();
	return true;
}


void RSSIChain::resetChain()
{
	if(myVerbose)
		printf("RSSIChain::resetChain --  resetting chain controller\n");

	chained_vehicles.clear();
	fc_mode = STANDBY;
	send_goto_wp = 0;

	if( shmPtr ) {
		Client * me = shmPtr->clients.getClient(0);
		if( me ) {
			if( me->telemetry.navigation.OBtype == OB_CHAIN || me->telemetry.navigation.OBtype == OB_NONE ) {
					me->telemetry.navigation.OBmode = OB_OFF;
					me->telemetry.navigation.OBtype = OB_NONE;
					me->telemetry.navigation.value  = NAN;
			}
		}
	}

	control_point.x = NAN;
	control_point.y = NAN;
	control_point.z = NAN;

	Vcp.x = 0.0;
	Vcp.y = 0.0;
	Vcp.z = 0.0;

	numClients = 0;
	for( int i=0; i<MAX_CLIENTS; i++) {
		clientRSSI_ind[i] = -1;
	}

}

void RSSIChain::update() 
{
	struct timeval timeout;
	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	Client * me = shmPtr->clients.getClient(0);
	bool haveGPSupdate = false;

	int val, maxFD;
	fd_set readFDs, writeFDs;                  // fd sets

	FD_ZERO(&readFDs);                         // zero read fd set
	FD_ZERO(&writeFDs);                        // zero write fd set
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);     // add fifo
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);    // add fifo

	maxFD = fifo_FDs[INBOARD_RX];
	if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("RSSIChain::update() select");
				execRate = -1;
			} else
				cout << getName() << "::update() - select interrupted" << endl;
		}
	}
	else {

		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) 
		{
			//printf("INBOARD_RX: ");
			int n = 0;
			n=readFifo(INBOARD_RX);
			if(n>0) {
				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length != fifo_msg.size) {
					cout << getName() << "::update() - error, length of command != length of buffer" << endl
						<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

					if( length < 1000 && length <= fifo_msg.size ){
						if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
							cout << "\tCMD: is valid" << endl;
						else
							cout << "\tCMD: is NOT valid" << endl;
					}

					return;
				}

				Command cmd((uint8_t *)fifo_msg.data);

				if( !cmd.isValid() )
					cout << getName() << "::update() - error, invalid command" << endl << cmd ;
				else {

					// lock the mutex while we access shared memory
					//mutex_lock();

					// client that sent the message
					Client * a_client;
					a_client = shmPtr->clients.getClient(fifo_msg.src);
					if(a_client != NULL) {

						uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

						switch(cmd.cmd() ) 
						{
							case CMD_CHAIN: 
							{
								int num_vehicles = cmd.size()/sizeof(VehicleInfo_t);

								// clear chain variables
								resetChain();

								cout << "Chain Controller: received chaining command for " << num_vehicles << " vehicles" << endl;

								for(int j=0;j<num_vehicles;j++) {
										// list of nodes in chaining command
										VehicleInfo_t* v = &(((VehicleInfo_t*)dataPtr)[j]);
										cout << "\ti=" << j << " id=" << v->id << endl;
										rssiClient[numClients].name = v->id;

										//-- see if we are given last byte of mac address
										if( v->id[0] ==':' ) {
											fc_mode = INITIALIZING;

											int mac;
											sscanf((v->id+1), "%x", &mac);
											printf("\t\tmac=0x%02X", mac);

											int z;
											for( z=0; z < MAX_CLIENTS; z++ ) {
												if( mac == me->linkstatus[z].mac[5] ) {
													printf(" -- clientInd=%i linkInd=%i", numClients, z);

													clientRSSI_ind[numClients] = z;
													numClients++;
													break;
												}
											}
											if( z == MAX_CLIENTS ) {
												cout << "!!! Could not find client using mac: " << v->id << " !!!" << endl;
												resetChain();
												break;
											}
											printf("\n");

										//-- see if we are given complete mac address
										} else if( strchr(v->id,':') ) {
											fc_mode = INITIALIZING;

											int mac[6];
											sscanf(v->id, "%x:%x:%x:%x:%x:%x",
													&mac[0],&mac[1],&mac[2],&mac[3],&mac[4],&mac[5]);
											printf("\t\tmacStr=%s --> mac=%02X:%02X:%02X:%02X:%02X:%02X", v->id, 
													mac[0],mac[1],mac[2],mac[3],mac[4],mac[5]);

											int z;
											for( z=0; z<MAX_CLIENTS; z++ ) {
												int sum=0;
												for( int iz=0; iz < 6; iz++ )
													if( mac[iz] == me->linkstatus[z].mac[iz] )
														sum++;

												if( sum == 6 ) {
													printf(" -- clientInd=%i linkInd=%i", numClients, z);

													clientRSSI_ind[numClients] = z;
													numClients++;
													break;
												}
											}
											if( z == MAX_CLIENTS ) {
												cout << "!!! Could not find client using mac: " << v->id << " !!!" << endl;
												resetChain();
												break;
											}
											printf("\n");

										//-- see if we are given complete ip address
										} else if (strchr(v->id,'.') ) {
											int ip[4];
											sscanf(v->id, "%i.%i.%i.%i", &ip[0],&ip[1],&ip[2],&ip[3]);
											printf("\t\tipStr=%s --> ip=%i.%i.%i.%i", v->id, ip[0],ip[1],ip[2],ip[3]);


											int z;
											for( z=0; z<MAX_CLIENTS; z++ ) {
												int sum=0;
												for( int iz=0; iz < 4; iz++ )
													if( ip[iz] == me->linkstatus[z].ip[iz] )
														sum++;

												if( sum == 4 ) {
													printf(" -- clientInd=%i linkInd=%i", numClients, z);

													clientRSSI_ind[numClients] = z;
													numClients++;
													break;
												}
											}
											if( z == MAX_CLIENTS ) {
												cout << "!!! Could not find client using ip: " << v->id << " !!!" << endl;
												resetChain();
												break;
											}	
											printf("\n");

#ifdef REMOTE_RSSI 
/*
											for(int i=0; i<4; i++)
												((uint8_t *)&(address.s_addr))[i]  = ip[i];

											Client *b_client = shmPtr->clients.getClient(address);
											if(b_client != NULL) {
												fc_mode = INITIALIZING;
												cout << "requesting TELEM stream" << endl;
												subscribe(CMD_LINK_STATUS, b_client, true, INBOARD_TX);
											} else {
												cout << "!!! Could not find client using ip: " << v->id << " !!!" << endl;
												resetChain();
												break;
											}									
*/ 
#endif

									//-- finally, see if we are given name
									} else {
										Client *b_client = shmPtr->clients.getClient(v->id);
										if(b_client != NULL) {
											fc_mode = INITIALIZING;

											in_addr addr = shmPtr->clients.getAddr(b_client);

											uint8_t ip[4]; 
											*((in_addr *)(ip)) = addr;
											printf("\t\tclient name=%s ip=%i.%i.%i.%i", b_client->name.c_str(), ip[0],ip[1],ip[2],ip[3]);

											int z;
											for( z=0; z<MAX_CLIENTS; z++ ) {
												int sum=0;
												for( int iz=0; iz < 4; iz++ )
													if( ip[iz] == me->linkstatus[z].ip[iz] )
														sum++;

												if( sum == 4 ) {
													printf(" -- clientInd=%i linkInd=%i", numClients, z);

													clientRSSI_ind[numClients] = z;
													numClients++;
													break;
												}
											}
											if( z == MAX_CLIENTS ) {
												cout << "!!! Could not find client using ip from name: " << v->id << " !!!" << endl;
												resetChain();
												break;
											}	else
												printf("\n");
#ifdef REMOTE_RSSI 
/*
											cout << "requesting TELEM stream" << endl;
											subscribe(CMD_LINK_STATUS, b_client, true, INBOARD_TX);
*/ 
#endif
										} else {
											cout << "!!! Could not find client using name or index: " << v->id << " !!!" << endl;
											resetChain();
											break;
										}									
									}

								}

								// only respond if valid initilization
								if( fc_mode == INITIALIZING && numClients == num_vehicles) {
									if( !(me->telemetry.navigation.OBtype == OB_NONE || me->telemetry.navigation.OBtype == OB_CHAIN) )  {
										NACK(INBOARD_TX);
										break;
									}

									ACK(INBOARD_TX);
									cout << "---------->>>>> Initializing chaining controller <<<<<----------" << endl;
									cout << "CHAINING " << numClients << " VEHICLES" << endl;
									me->telemetry.navigation.OBmode = OB_INIT;
									me->telemetry.navigation.OBtype = OB_CHAIN;
									send_goto_wp = 1;

								} else {
									// send a NACK back
									if( num_vehicles > 0 ) NACK(INBOARD_TX);
									else if( num_vehicles == 0 ) ACK(INBOARD_TX);
								}

							} break;

							case CMD_ACK: {
									//cout << "CMD_ACK from " << a_client->name << endl;
									break;
								} 
							case CMD_FAIL: {
									// we should do something
									//cout << "CMD_FAIL from " << a_client->name << endl;
									break;
								};
							case CMD_RETURN: {
									if( fc_mode != STANDBY ) {
										printf(" stopping chaining controller\n");
										resetChain();
									}
									printf("\n");
									break;
								} 
	
							case CMD_LINK_STATUS: {
									 if( me->name == a_client->name ) 
										 break;

									 int num_rssi_val = cmd.size()/sizeof(LinkStatus_t);
									 cout << "CMD_LINK_STATUS: from=" << a_client->name << " num_rssi_val=" << num_rssi_val << endl;
#ifdef REMOTE_RSSI 
/*
									 cout << "CMD_LINK_STATUS: from=" << a_client->name << " num_rssi_val=" << num_rssi_val << endl;
									 for( int ii=0; ii < num_rssi_val; ii++) {

										 LinkStatus_t * link	= &((LinkStatus_t *)dataPtr)[ii];
										 in_addr address;

										 for(int i=0; i<4; i++)
											 ((uint8_t *)&(address.s_addr))[i]  = link->ip[i];


										 // this is who sent me the message
										 //Client * chained_client = chained_vehicles.getClient(a_client->name);
										 // this is which client was tracked
										 Client * chained_client = chained_vehicles.getClient(address);

										 if(chained_client != NULL) {
											 //chained_client->linkstatus = *link;

											 int ind;
											 bool setCount = 0;
											 for( ind=0; ind<MAX_CLIENTS; ind++)
												 if( rssiClient[ind].name == chained_client->name ){
													 int r_ind = rssiClient[ind].useIND;
													 rssiClient[ind].link[r_ind]  = *link;
													 rssiClient[ind].rssi[r_ind]  = link->signal;
													 //rssiClient[ind].rssi[r_ind]  = link->signal - link->noise;
													 rssiClient[ind].myGPS[r_ind] = me->telemetry.gps.pos;
#if 0
													 if( r_ind > 0 ) {
														 rssiClient[ind].delta_RSSI[r_ind] = rssiClient[ind].rssi[r_ind] - rssiClient[ind].rssi[r_ind-1];
														 rssiClient[ind].delta_LatLon[r_ind].lat = rssiClient[ind].myGPS[r_ind].lat - rssiClient[ind].myGPS[r_ind-1].lat;
														 rssiClient[ind].delta_LatLon[r_ind].lon = rssiClient[ind].myGPS[r_ind].lon - rssiClient[ind].myGPS[r_ind-1].lon;
													 } else if( r_ind == 0 ) {
														 rssiClient[ind].delta_RSSI[r_ind] = rssiClient[ind].rssi[r_ind] - rssiClient[ind].rssi[NUM_RSSI-1];
														 rssiClient[ind].delta_LatLon[r_ind].lat = rssiClient[ind].myGPS[r_ind].lat - rssiClient[ind].myGPS[NUM_RSSI-1].lat;
														 rssiClient[ind].delta_LatLon[r_ind].lon = rssiClient[ind].myGPS[r_ind].lon - rssiClient[ind].myGPS[NUM_RSSI-1].lon;
													 }
#endif
													 rssiClient[ind].useIND = ++r_ind % MAX_NUM_RSSI;

													 setCount++;
												 }
											 if( setCount == 0 || setCount != chained_vehicles.getNumClients() )
												 cout << "---> CMD_LINK_STATUS did not process all of the chaining nodes" << endl;

										 } else
											 printf("RSSIChain::update -- did not find [%s] in link status link\n", chained_client->name.c_str());
									 }
*/ 
#endif
								 } break;

							case CMD_CHAIN_GAINS: {
									CopyBufToStruct(dataPtr, gains);

									//if( gains.vmax < 0 )  gains.vmax = -gains.vmax;
									//if( gains.vmax == 0 ) gains.vmax = ORBIT_SPEED;
									//if( gains.kv < 0 )    gains.kv = -gains.kv;
									//gains.kv = -gains.kv;
									//if( gains.kv == 0 )   gains.kv = DEFAULT_GAIN_VAL;

									controlMode = (ControllerMode)gains.mode;

									//printf( "Gains: vmax=%f kv=%f mode=%i\n", gains.vmax, gains.kv, gains.mode);

									// reply back that we got message
									ACK(INBOARD_TX);
								} break;

							case CMD_ORBIT_VALUES: {
									CopyBufToStruct(dataPtr, orbit);

									//printf( "Orbit: rad=%f alt=%f wp=%i dir=%i\n", orbit.rad, orbit.alt, orbit.wp, orbit.dir);

									// reply back that we got message
									ACK(INBOARD_TX);
								} break;

							case CMD_FP: {
									// somebody else sent a waypoint plan, give up control
									//writeFifo(OUTBOARD_TX);
									if( a_client != me && me->telemetry.navigation.OBtype == OB_CHAIN) {
										printf("----> CMD_FP (from %s): stopping chaining controller <----\n", a_client->name.c_str() );
										resetChain();
									}

								} break;

							case CMD_TELEM: {
								int r_ind = useIND;
								useIND    = (++useIND) % MAX_NUM_RSSI;

								// convert coordinates to ECEF
								ecef[r_ind] = lla2ecef( me->telemetry.gps.pos );

								// set gps flag to update controller
								haveGPSupdate = true;

								// update RSSI values for all clients
								for( int ind=0; ind < MAX_CLIENTS; ind++) {
									rssiClient[ind].link[r_ind] = me->linkstatus[ ind ];
								}

								break;
							}
							default:
								//printf("--> CHAIN UNHANDLED CMD: cmd=0x%2x\n",cmd.cmd());
								//printf("--> OUTBOARD_TX: cmd=0x%2x\n",cmd.cmd());
								//writeFifo(OUTBOARD_TX);
								break;
						}
					}
					// unlock shared memory
					//mutex_unlock();
				}
			}
		}
	}

	// loop time
	currentStamp.stamp();
	dT = (currentStamp - last_chaining_loop) / SEC2MICRO;

	// Check to make sure we are running and data is good
	if( fc_mode > INITIALIZING && dT > 10.0 )  {
		cout << "!! Chaining - timed out waiting for updates, giving up control" << endl;
		// initialize chain variables
		resetChain();
	} 
	else if(haveGPSupdate) 
	{
		last_chaining_loop = currentStamp; // keeps timing more accurate

		//-- Check for proper initialization of buffers
		if( IsNaN(ecef[MAX_NUM_RSSI-1].x) || fc_mode == STANDBY || IsNaN(control_point.x) ) {
			//-- track the control point as simply the average over what we have
			int r_ind = ( IsNaN(ecef[MAX_NUM_RSSI-1].x) ? useIND : MAX_NUM_RSSI );

			control_point.x = 0;
			control_point.y = 0;
			control_point.z = 0;
			for( int z=0; z < r_ind; z++ ) {
				control_point.x += ecef[z].x / (double)r_ind;
				control_point.y += ecef[z].y / (double)r_ind;
				control_point.z += ecef[z].z / (double)r_ind;
			}

			// convert ECEF control point to geodectic GPS
			orbit_point = ecef2lla(control_point);

			//if(myVerbose)
				//printf("Chaining - waiting to fill buffers :\n\tr_ind=%i last_rx=%f\n\tlon=%f lat=%f alt=%f\n",
						//r_ind, rssiClient[0].link[r_ind-1].last_rx, orbit_point.lon, orbit_point.lat, orbit_point.alt);
		} 
		
		//-- run the chain targets function if in a running mode
		if( fc_mode > STANDBY && !IsNaN(ecef[MAX_NUM_RSSI-1].x)) {
			currentStamp.stamp();
			orbit_dT = (currentStamp - last_orbit_ts) / SEC2MICRO;
			if( orbit_dT >= CHAINING_LOOP_PERIOD ) {
				chainTargets(); 
				last_orbit_ts = currentStamp;

				// Send goto wp command
				if(send_goto_wp){
					GotoWP_t wp;
					wp.ind = orbit.wp;
					CMD_CMD(fifo_msg.data)  = CMD_GOTO_WP;
					CMD_NUM(fifo_msg.data)  = sizeof(GotoWP_t);
					CopyStructToBuf(wp, CMD_DATAPTR(fifo_msg.data) );
					CMD_CSUM(fifo_msg.data) = 0;
					CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

					fifo_msg.size = CMD_SIZE(fifo_msg.data);
					fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr;
					fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
					writeFifo(INBOARD_TX);
					send_goto_wp = 0;
				}

			}
		}

		//-- log the data
		if(logging) 
		{
			logfile=fopen(log_name.c_str(), "a");
			if(logfile != NULL) {
				update_gps_str();
				fprintf(logfile,"%s\n", gpsStr);
				fprintf(logfile,"- %i, %f, %f, %f, %f, %f, %i -\n",
						fc_mode,
						orbit_point.lat,
						orbit_point.lon,
						Vcp.x,
						Vcp.y,
						Vcp.z,
						numClients);

				int r_ind = useIND - 1;
				for(int j=0; j < numClients; j++) {
					fprintf(logfile,"%i, %s, %f, %f, %f, %f, %f\n",
							j, 
							rssiClient[clientRSSI_ind[j]].name.c_str(),
							rssiClient[clientRSSI_ind[j]].avgRSSI,
							rssiClient[clientRSSI_ind[j]].rssiVEC.x,
							rssiClient[clientRSSI_ind[j]].rssiVEC.y,
							rssiClient[clientRSSI_ind[j]].rssiVEC.z,
							rssiClient[clientRSSI_ind[j]].link[r_ind].last_rx);
				}

				fprintf(logfile,"\n");
				// close file
				fflush(logfile);
				fclose(logfile);
			}
		}

	} // haveGPSupdate
}

void RSSIChain::chainTargets() 
{
	double norm;
	Client * me = shmPtr->clients.getClient(0);
	isValidControll = false;

	if( fc_mode == INITIALIZING ) {
		fc_mode = CHAINING;
		cout << "---------->>>>> Executing chaining controller <<<<<----------" << endl;
	}

	// reset the total control vector
	esVEC.x = 0;
	esVEC.y = 0;
	esVEC.z = 0;

	//-----------------------------------------//
	//-       Placket-Burman estimation       -//
	//-----------------------------------------//    
	DoE_gradEst();

	// check for valid estimation
	if( !isValidControll ) {
		me->telemetry.navigation.OBmode = OB_ERROR;
		return;
	}

	//-----------------------------------------//
	//--        Controller            -//
	//-----------------------------------------//
	if( controlMode == MAXIMIN_CONTROLLER )
		maximin_controller();
	else if( controlMode == RR_CONTROLLER )
		rr_controller();

	// check for valid controll
	if( !isValidControll || IsNaN(esVEC.x) || IsNaN(esVEC.y) || IsNaN(esVEC.z) ) {
		me->telemetry.navigation.OBmode = OB_ERROR;
		return;
	}

	//--------------------------------------------//
	//-- Point Mass Bounded Velocity Controller --//
	//--------------------------------------------//
	Vcp.x = gains.kv * esVEC.x;
	Vcp.y = gains.kv * esVEC.y;
	Vcp.z = gains.kv * esVEC.z;

	//-- upper bound the update vector with maximum velocity
	norm = sqrt(Vcp.x*Vcp.x + Vcp.y*Vcp.y + Vcp.z*Vcp.z);
	if( norm > gains.vmax ) {
		Vcp.x /= norm / gains.vmax;
		Vcp.y /= norm / gains.vmax;
		Vcp.z /= norm / gains.vmax;
	}

	// now that we have a velocity vector, move the control point 
	control_point.x += Vcp.x * orbit_dT;
	control_point.y += Vcp.y * orbit_dT;
	control_point.z += Vcp.z * orbit_dT;

	// convert ECEF control point to geodectic GPS
	orbit_point = ecef2lla(control_point);

	if(myVerbose) 
		printf( "Vcp=%f [m/s] : x=%f y=%f z=%f\n", norm, Vcp.x, Vcp.y, Vcp.z);

	//-----------------------------------------//
	//--          Orbit Controller            -//
	//-----------------------------------------//
	command_orbit_controller();
}

/*
		// calc motion vector based on difference of max to min

		if( numClients == 1 ) maxRSSI = 0;
		norm = sqrt( esVEC.lat*esVEC.lat + esVEC.lon*esVEC.lon );
		if( norm > 0 ) {
			esVEC.lat = gains.kv * ( maxRSSI - minRSSI ) * esVEC.lat / norm; 
			esVEC.lon = gains.kv * ( maxRSSI - minRSSI ) * esVEC.lon / norm; 
		}
*/

// maximize minimum SNR value, use average as surrogate for Least-Norm
void RSSIChain::maximin_controller() 
{
	Client * me = shmPtr->clients.getClient(0);

	// find client with minimum avgRSSI
	int minInd = 0;
	float minRSSI = rssiClient[clientRSSI_ind[0]].avgRSSI;
	float maxRSSI = minRSSI;
	for(int j=1; j < numClients; j++)  {
		if( rssiClient[clientRSSI_ind[j]].avgRSSI < minRSSI ) {
			minInd = j;
			minRSSI = rssiClient[clientRSSI_ind[j]].avgRSSI;
		} else if ( rssiClient[clientRSSI_ind[j]].avgRSSI > maxRSSI ) {
			maxRSSI = rssiClient[clientRSSI_ind[j]].avgRSSI;
		}
	}

	if( !IsNaN( rssiClient[clientRSSI_ind[minInd]].avgRSSI ) ) {
		isValidControll = true;

		// set the value we are operating on
		me->telemetry.navigation.value = rssiClient[clientRSSI_ind[minInd]].avgRSSI;

		// calc performane function gradient vector 
		esVEC.x = rssiClient[clientRSSI_ind[minInd]].rssiVEC.x;
		esVEC.y = rssiClient[clientRSSI_ind[minInd]].rssiVEC.y;
		esVEC.z = rssiClient[clientRSSI_ind[minInd]].rssiVEC.z;
	} else {
		isValidControll = false;
		me->telemetry.navigation.value = NAN;
		esVEC.x = 0.0;
		esVEC.y = 0.0;
		esVEC.z = 0.0;
	}

	if(myVerbose)
		printf("MaxiMin Vector: min=%i x=%g y=%g z=%g value=%f mode=%i\n", 
			minInd, esVEC.x, esVEC.y, esVEC.z, me->telemetry.navigation.value, me->telemetry.navigation.OBmode );
}

// Minimize the end-to-end delay
void RSSIChain::rr_controller() 
{
	Client * me = shmPtr->clients.getClient(0);

	// find client with minimum avgRSSI
	double C = {0.0};
	double gradC = {0.0};
	double gradT = {0.0};

	// set the value we are operating on
	isValidControll = true;
	esVEC.x = 0;
	esVEC.y = 0;
	esVEC.z = 0;
	me->telemetry.navigation.value = 0;

	for(int j=0; j < numClients; j++)  {
		double SNR = rssiClient[clientRSSI_ind[j]].avgRSSI - MIN_RSSI; 
		if( SNR > So ) {
			C     = Cmax * (1 - exp( -Ae*(SNR - So) ) );
			gradC = Ae * Cmax * exp( -Ae*(SNR - So) ) ;
			gradT = - gradC / (C*C);

			if(myVerbose)
				printf("SNR=%f So=%f Ae=%f Cmax=%f C=%f gradC=%f gradT=%f\n",
				        SNR, So, Ae, Cmax, C, gradC, gradT);

			// note, since we want to minimize the delay we have to
			// add in negative sign
			esVEC.x -= gradT * rssiClient[clientRSSI_ind[j]].rssiVEC.x;
			esVEC.y -= gradT * rssiClient[clientRSSI_ind[j]].rssiVEC.y; 
			esVEC.z -= gradT * rssiClient[clientRSSI_ind[j]].rssiVEC.z; 

			// set the value we are operating on
			me->telemetry.navigation.value += 1 / C;

		} else {
			isValidControll = false;
			esVEC.x = 0.0;
			esVEC.y = 0.0;
			esVEC.z = 0.0;
			return;
		}
	}

	if(myVerbose)
		printf("RR Vector: x=%g y=%g z=%g value=%g mode=%i\n", 
			esVEC.x, esVEC.y, esVEC.z, me->telemetry.navigation.value, me->telemetry.navigation.OBmode );
}

// with new orbit point, give orbital controller update
void RSSIChain::command_orbit_controller() 
{
	if(myVerbose) 
		printf( "New Orbit Point: lat=%f lon=%f alt=%f\n", orbit_point.lat, orbit_point.lon, orbit_point.alt);

#ifdef USE_AUTOPILOT_ORBIT
	/*----------[ using the autopilot to orbit ]----------*/
	Client * me = shmPtr->clients.getClient(0);
	me->telemetry.navigation.OBmode = OB_ORBIT;

	Waypt_t * waypoint = (Waypt_t*)CMD_DATAPTR(fifo_msg.data);
	waypoint[0].number   = orbit.wp;
	waypoint[0].pos.lat  = orbit_point.lat;
	waypoint[0].pos.lon  = orbit_point.lon;
	waypoint[0].pos.alt  = orbit.alt;
	waypoint[0].orbitRad = (int16_t)(orbit.dir * orbit.rad);
	waypoint[0].next     = orbit.wp + 1;

	waypoint[1].number   = orbit.wp + 1;
	waypoint[1].pos.lat  = me->telemetry.gps.pos.lat;
	waypoint[1].pos.lon  = me->telemetry.gps.pos.lon;
	waypoint[1].pos.alt  = orbit.alt;
	waypoint[1].orbitRad = 0;
	waypoint[1].next     = orbit.wp;

	CMD_CMD(fifo_msg.data)  = CMD_FP;
	CMD_NUM(fifo_msg.data)  = 2*sizeof(Waypt_t);
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

#else
	/*----------[ LGVF controller to orbit ]----------*/
	me->telemetry.navigation.OBmode = OB_TURNRATE;

	float psides;
	float psidotdes;

	vectorField(me->telemetry.gps.pos, me->telemetry.gps.track, 
			orbit_point, orbit.rad, me->telemetry.airdata.tas, &psides, &psidotdes);

	APRates_t * ap_rates = (APRates_t*)CMD_DATAPTR(fifo_msg.data);

	ap_rates->heading  = psides;
	ap_rates->vel_rate = 0;

	CMD_CMD(fifo_msg.data)  = CMD_AP_RATES;
	CMD_NUM(fifo_msg.data)  = sizeof(APRates_t);
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);
#endif

	// send the command to myself, for the thread that handles piccolo interface
	fifo_msg.size        = CMD_SIZE(fifo_msg.data);
	fifo_msg.src.s_addr  = shmPtr->clients.getAddr(me).s_addr;
	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr;

	//cout << "SENDING NEW WAYPOINT" << endl;

	writeFifo(INBOARD_TX);
}

#define NUM_DIM_DOE 2
//--------------------------------------
// Design-of-Experiments Gradient Estimation
//--------------------------------------    
void RSSIChain::DoE_gradEst()
{
	isValidControll = true;

	// loop over all clients
	for(int clientInd=0; clientInd < numClients; clientInd++) 
	{
		int  minN    = 10;
		int  cInd    = minN;
		bool notDone = 1;

		// get the last time
		int r_ind = useIND - 1;
		if( r_ind < 0 ) r_ind = MAX_NUM_RSSI-1;

		Matrix XX;    // Hadamard Matrix
		Matrix f;     // Sampled rssi values
		Matrix C_e_n; // ECEF to NED DCM

		while(notDone && cInd < MAX_NUM_RSSI) {
			// temp matricies
			Matrix X(3,cInd); // position state vector
			Matrix Xavg(3,1); // average of position state

			// zero out old/used matricies
			XX = Matrix(cInd, NUM_DIM_DOE+1);
			f  = Matrix(cInd,1);

			// iterate over sample range
			int tInd = r_ind;
			int stopInd = tInd+1;
			for( int z=0; z < cInd; z++ ) {
				// find a good data point
				while( rssiClient[clientRSSI_ind[clientInd]].link[tInd].last_rx > 0.95 ) {
					cout << "*" << endl;
					// decreement and loop sample index
					if( --tInd < 0 ) tInd = MAX_NUM_RSSI-1; 
					if( tInd == stopInd ) {
						break;
					}
				}

				// never found good data for client
				if( tInd == stopInd ) {
					cout << "!! Chaining - do not have enough valid rssi measurements" << endl;
					rssiClient[clientRSSI_ind[clientInd]].avgRSSI = NAN;
					cInd    = 0;
					notDone = false;
					continue;
				}
					
				// get coordinates
				X(0,z) = ecef[tInd].x;
				X(1,z) = ecef[tInd].y;
				X(2,z) = ecef[tInd].z;

				// sum total for average, we do the division here so as
				// to not sum large numbers, which ECEF coordinates are
				Xavg(0) += X(0,z) / (double)cInd;
				Xavg(1) += X(1,z) / (double)cInd;
				Xavg(2) += X(2,z) / (double)cInd;

				// rssi signal value
				f(z) = rssiClient[clientRSSI_ind[clientInd]].link[tInd].signal;

				// decreement and loop sample index
				if( --tInd < 0 ) tInd = MAX_NUM_RSSI-1; 
			}

			double theta0 = NAN;
			double theta1 = NAN;
			if( cInd > 0 ) { 
				// calculate radial distance and angles
				Matrix theta(1,cInd);

				// convert ECEF average point to geodectic 
				Matrix avg_point = ecef2lla(Xavg);

				// get ECEF to NED DCM at avg_point location
				C_e_n = ecef2nedDCM( avg_point );

				for( int z=0; z < cInd; z++ ) {

					// perturbation vector, position of UA relative to average in
					// ECEF coordinate frame
					Matrix R(3,1); 
					R(0) = X(0,z) - Xavg(0);
					R(1) = X(1,z) - Xavg(1);
					R(2) = X(2,z) - Xavg(2);

					// coordinate transformation to NED
					R = C_e_n * R;

					if( IsNaN(R(2)) )
						printf("R(2)=%f\n", R(2) );

					// angle coverage of perturbations (aka right ascension)
					theta(0,z) = atan2( R(1), R(0) );

					// Hadamard matrix
					XX(z,0) = 1;
					XX(z,1) = R(0);
					XX(z,2) = R(1);
					if( NUM_DIM_DOE > 2 )
						XX(z,3) = R(2);
				}

				theta0 = theta(0,cInd-1) - theta(0,0);
				theta0 += ( theta0 < -M_PI ? PI_2 : 0 );
				theta0 -= ( theta0 >  M_PI ? PI_2 : 0 );
				
				theta1 = theta(0,cInd-2) - theta(0,0);
				theta1 += ( theta1 < -M_PI ? PI_2 : 0 );
				theta1 -= ( theta1 >  M_PI ? PI_2 : 0 );

				//printf("Angles: theta0=%f theta=%f dTheta=%f, %f\n", 
						//theta(0), theta(0,cInd-1), theta0, theta1);

				//-- Check for complete circle obtained --//
				// because we use the average, we can get to a point where we actually
				// don't have the zero crossing at the last sample, so check a few
				// samples back in time for a zero crossing
				for( int zI=0; zI < cInd - 3; zI++ ) {
					if( SIGN(theta0) + SIGN(theta1) == 0 && fabs(theta1-theta0) < M_PI ) {
						//cout << "Complete circle: n=" << cInd << endl;
						notDone = false;
						break;
					}
				}

				// increment the search time
				cInd++;
			}
		}

		if( myVerbose) 
			cout << endl << "Number of Samples = " << cInd << endl;

		if( cInd > 0 ) {
			// now that we have our center point and perturbation
			// vectors, we can run the DoE vector estimation
			Matrix temp = ~XX * XX;
			if( temp.IsSingular() ) {
				rssiClient[clientRSSI_ind[clientInd]].avgRSSI = NAN;
				isValidControll = false;
				return;
			} else {
				Matrix pb = temp.Inv() * ~XX * f;

				if( myVerbose ) {
					printf("PB: f_avg=%f x=%f y=%f ",pb(0),pb(1),pb(2));
					if( NUM_DIM_DOE > 2 ) printf("z=%f",pb(3));
					printf("\n");
				}

				// get gradient vector values
				Matrix R(3,1);
				R(0) = pb(1);
				R(1) = pb(2);
				R(2) = 0.0;  // zero the down component
				if( NUM_DIM_DOE > 2 ) R(2) = pb(3);

				// now convert NED grad vector back to ECEF frame
				R = ~C_e_n * R;

				// save values 
				rssiClient[clientRSSI_ind[clientInd]].avgRSSI     = pb(0);
				rssiClient[clientRSSI_ind[clientInd]].rssiVEC.x   = R(0);
				rssiClient[clientRSSI_ind[clientInd]].rssiVEC.y   = R(1);
				rssiClient[clientRSSI_ind[clientInd]].rssiVEC.z   = R(2);
			}
		}
	} // next client
}
